//#include <Wire.h>
#include <Servo.h>
#include <Usb.h>
#include <AndroidAccessory.h>
#define  RADIO1         2
#define  RADIO2         3
#define  RADIO3         4
#define  RADIO4         5
#define  RADIO5         7
#define  RADIO6         6

#define  SERVO1         9
#define  SERVO2         10
#define  SERVO3         11
#define  SERVO4         12
#define  SERVO5         13
//int varX = 1;
//int val = 0; 
#define channumber 6
#define filter 8 // Filtro anti salto/ Glitch Filter
int channel[channumber]; //Valores de canales leidos/ readed Channel values
int lastReadChannel[channumber]; //Ultima lectura obtenida/ Last  values readed  
//int conta=0; //Contador/couter  
//int PPMin = 6;
Servo servos[5];
int radio[6];
int boli=0;


AndroidAccessory acc("Google, Inc.",
		     "FlyingDroid",
		     "DemoKit Arduino Board",
		     "1.0",
		     "http://code.google.com/p/flyingdroid/",
		     "0000000012345678");
// 10M ohm resistor on demo shield
//CapSense   touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV);

void setup();
void loop();

void init_inputs()
{
        radio[0]=RADIO1;
        radio[1]=RADIO2;
        radio[2]=RADIO3;
        radio[3]=RADIO4;
        radio[4]=RADIO5;
        radio[5]=RADIO6;
        pinMode(RADIO1, INPUT);
	pinMode(RADIO2, INPUT);
	pinMode(RADIO3, INPUT);        
	pinMode(RADIO4, INPUT);
	pinMode(RADIO5, INPUT);
        pinMode(RADIO6, INPUT);
}

void setup()
{
	Serial.begin(115200);
	Serial.print("\r\nStart");
	init_inputs();
	
	servos[0].attach(SERVO1);
	servos[0].write(90);
	servos[1].attach(SERVO2);
	servos[1].write(90);
	servos[2].attach(SERVO3);
	servos[2].write(0.5);
        servos[3].attach(SERVO4);
	servos[3].write(90);
	servos[4].attach(SERVO5);
	servos[4].write(90);

	acc.powerOn();
}

void loop()
{
	byte err;
	byte idle;
        
	static byte count = 0;
	byte msg[3];
	long touchcount;

	if (acc.isConnected()) {
		int len = acc.read(msg, sizeof(msg), 1);
		int i;
		byte b;
		uint16_t val;
		int x, y;
		char c0;

		if (len > 0) {
			// assumes only one command per packet
			if (msg[0] == 0x2) {
                                //Serial.print(msg[1]);
//				if (msg[1] == 0x0)
//					analogWrite(LED1_RED, 255 - msg[2]);
//				else if (msg[1] == 0x1)
//					analogWrite(LED1_GREEN, 255 - msg[2]);

				 if (msg[1] == 0x8)
					servos[0].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0x9)
					servos[1].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0xA)
					servos[2].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0xB)
					servos[3].write(map(msg[2], 0, 255, 0, 180));
				else if (msg[1] == 0xC)
					servos[4].write(map(msg[2], 0, 255, 0, 180));
//			} else if (msg[0] == 0x3) {
//				if (msg[1] == 0x0)
//					digitalWrite(RELAY1, msg[2] ? HIGH : LOW);
//				else if (msg[1] == 0x1)
//					digitalWrite(RELAY2, msg[2] ? HIGH : LOW);
			}
		}

		msg[0] = 0x1;
//
//		b = digitalRead(BUTTON1);
//		if (b != b1) {
//			msg[1] = 0;
//			msg[2] = b ? 0 : 1;
//			acc.write(msg, 3);
//			b1 = b;
//		}
//

		switch (count++ % 0x10) {
		case 0:
			val = analogRead(RADIO1);
			msg[0] = 0x4;
			msg[1] = val >> 8;
			msg[2] = val & 0xff;
			acc.write(msg, 3);
			break;

		case 0x4:
			val = analogRead(RADIO2);
			msg[0] = 0x5;
			msg[1] = val >> 8;
			msg[2] = val & 0xff;
			acc.write(msg, 3);
			break;
		}
	} else {		
if (pulseIn(radio[1] , HIGH) > 900) //Si el pulso del pin 4 es > que 4 msegundos continua /If pulse > 4 miliseconds, continues  
  {   
   
  for(int i = 0; i <= channumber-1; i++) //Promedia los pulsos/Average the pulses  
    {  
      channel[i]=pulseIn(radio[i], HIGH); 
    if((channel[i] > 2000) || (channel[i] < 1000))//Si se pasa del rango envia ultimo pulso/ If channel > max range, chage the value to the last pulse  
    { 
       channel[i]=lastReadChannel[i];
    }  
    else  
    { 
     channel[i]=channel[i]-1000;
     channel[i]=channel[i]/5;        
      if (lastReadChannel[i]>=channel[i])
      {
      if (lastReadChannel[i]-channel[i]>filter)
      {        
        servos[i].write((lastReadChannel[i]+channel[i])/2);
      } 
      } else {
        if (channel[i]-lastReadChannel[i]>filter)
      {
        servos[i].write((lastReadChannel[i]+channel[i])/2);
      } 
      }  
      lastReadChannel[i]=(lastReadChannel[i]+channel[i])/2;
    }
    } 
  } 
  
 }     
	
}

